% 进动运动学模拟器
% Gitee Repo

clc
clear

dt = 0.05;
w = 2*pi/0.5; %自转角速度
W = 2*pi/1; %自转轴进动角速度


R = 1; %球半径
_R = 3;
tiltangle = pi/6; %自转轴与进动轴倾角

orbitalh = _R*cos(tiltangle);
orbitalR = _R*sin(tiltangle);

function h = SPHERE(x,y,z,r)
  [X,Y,Z] = sphere(20);
  h = surf(r*X+x,r*Y+y,r*Z+z);
  set(h,'EdgeColor','none')
  set(h,'CData',X)
end

figure
for tick = 1:100
    t = tick * dt;

    theta = w*t;
    THETA = W*t;

    clf
    hold on
    axis equal

    view([30,45])
    axis([-5,5,-5,5,-5,5])

    h = SPHERE(0,0,_R,R);
    rotate(h,[0,0,1],theta/pi*180,[0,0,0]); %自转
    rotate(h,[0,1,0],tiltangle/pi*180,[0,0,0]); %进动倾角
    rotate(h,[0,0,1],THETA/pi*180,[0,0,0]); %进动

    _t = 0:0.02:2*pi;
    _x = orbitalR*cos(W*_t);
    _y = orbitalR*sin(W*_t);
    _z = orbitalh*ones(size(_x));
    plot3(_x,_y,_z); %轨道

    spherecoord =[orbitalR*cos(THETA);orbitalR*sin(THETA);orbitalh];
    spherevec = spherecoord/norm(spherecoord);

    line([0,spherecoord(1)],[0,spherecoord(2)],[0,spherecoord(3)])
    quiver3(spherecoord(1),spherecoord(2),spherecoord(3),2*R*spherevec(1),2*R*spherevec(2),2*R*spherevec(3)) %角速度方向

    line([0,0],[0,0],[0,orbitalh]);

    drawnow
    pause(0.01);

end

